pcloud = depthTo2DCloud(depth_image);

subplot(223)
imshow(depth_image, [500, 1000])
colormap('jet');

plan = ransacPlan(pcloud, 6, 0.05);
subplot(222);
plot(plan(:,1), plan(:,2), 'x');
axis equal
subplot(221);
plot(pcloud(:,1), pcloud(:,2), '.r');
%plan(:,3) = 1;

theta = 0.1;

%rotation = [cos(theta), -sin(theta), 0; sin(theta), cos(theta), 0; 0, 0, 1];
rotation = [cos(theta), -sin(theta); sin(theta), cos(theta)]
plan2 = plan * rotation;
[r, t] = icp(plan, plan2)
subplot(224)
plot(plan2(:,1), plan2(:,2), '.r')
axis equal
